package org.scalpel.locodo;

import org.scalpel.common.HHObject;
import org.scalpel.common.KeyPoint;


 
/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * The class Odometry helps to compute current position 
 * of the robot in the field space. It uses robot's geometric constants
 * such as encoder wheel perimeter, or distance between encoder wheel 
 * and the robot center...
 * It also helps to compute relative position of a target in the 
 * field space.
 */
public class Odometry extends HHObject {
  
  
  public Odometry( String name,  double encoderWheelDiameter, 
            double encoderWheelsDistanceToCenter,
            int encoderTickPerRotation
           ) {
    super(name);
    
    fEncoderWheelDiameter = encoderWheelDiameter;
    fEncoderWheelPerimeter = Math.PI * fEncoderWheelDiameter;

    fEncoderWheelsDistanceToCenter  = encoderWheelsDistanceToCenter;
    fEncoderTickPerRotation = encoderTickPerRotation;
    fEncLeft_prev   = 0;
    fEncRight_prev  = 0;
    fCurrPosition = new KeyPoint();
  }

  
  /**
   * setPosition : set the current position of the robot
   * @param position Position in the field space.
   */
  public synchronized void setPosition( KeyPoint position ){ 
    fCurrPosition = position; 
  }


  /**
   * getPosition : get the current position of the robot
   * @return Current position in the field space.
   */
  public synchronized KeyPoint getPosition(){ 
    return fCurrPosition; 
  };

  
  /**
   * update position according to the robot geometry and current encoder values
   * need frequent update to keep high precision
   * @param currLeftEncoderValue Left encoder value
   * @param currRightEncoderValue Right encoder value
   * @return return current field position
   */
  synchronized KeyPoint updatePosition(int currLeftEncoderValue, int currRightEncoderValue) {
    /**
     * compute the difference between the last and current encoder's position
     */
    double deltaRightMeter = ((double)(currRightEncoderValue - fEncRight_prev) 
                              / (double)fEncoderTickPerRotation
                            ) * fEncoderWheelPerimeter;
                            
    double deltaLeftMeter = ((double)(currLeftEncoderValue - fEncLeft_prev)
                              / (double)fEncoderTickPerRotation
                            ) * fEncoderWheelPerimeter;

    /**
     * Save the new encoders values
     */
    fEncRight_prev = currRightEncoderValue;
    fEncLeft_prev =  currLeftEncoderValue;

    /**
     * compute the difference between the two wheels and
     * compute the middle of the two wheels.
     * difference is used to compute rotational component of the current motion
     * mean is used to compute the straight component
     */
    double deltaRoue =  deltaRightMeter - deltaLeftMeter;
    double meanRoue  = (deltaRightMeter + deltaLeftMeter) / 2.0f;

    /**
     * compute the angle between the last direction and the current direction of robot
     */
    //double deltaAngle = deltaRoue / (2.0f * fEncoderWheelsDistance); //hypothesis : little angles
    double deltaAngle = Math.atan2(deltaRoue , (2.0f * fEncoderWheelsDistanceToCenter));

    deltaAngle = (deltaAngle % Math.PI);
    if( deltaAngle > Math.PI ) deltaAngle -= 2.0f * Math.PI;
    if( deltaAngle < -Math.PI ) deltaAngle += 2.0f * Math.PI;

    fCurrPosition.a += deltaAngle;
    if( fCurrPosition.a >  Math.PI ) fCurrPosition.a -= 2.0f * Math.PI;
    if( fCurrPosition.a < -Math.PI ) fCurrPosition.a += 2.0f * Math.PI;

    // compute cartesian position of robot
    fCurrPosition.x += Math.cos( fCurrPosition.a) * meanRoue;
    fCurrPosition.y -= Math.sin( fCurrPosition.a) * meanRoue;

    // compute the distance to the zero point
    fCurrPosition.d = Math.sqrt( fCurrPosition.x * fCurrPosition.x + fCurrPosition.y * fCurrPosition.y);
    
    return fCurrPosition;
  }

  /**
   * update position according to the robot geometry and current encoder values
   * @param target position of a target in the field space
   * @return relative position of the target in the robot space
   */
  KeyPoint getRelativeTargetPosition( KeyPoint target ){
    return getRelativeTargetFromPosition( fCurrPosition, target );
  }
  
  KeyPoint getRelativeTargetFromPosition( KeyPoint refSpace, KeyPoint target )
  {
    // relative position of the target
    KeyPoint relT = new KeyPoint();

    ////////////
    // The target point is define in the referential space using the homogeneous coordinates
    ////////////

    double xTmp = target.x - refSpace.x;
    double yTmp = target.y - refSpace.y;
    
    double xTarget_rob = (Math.cos( refSpace.a ) * xTmp - Math.sin( refSpace.a ) * yTmp);
    double yTarget_rob = (Math.sin( refSpace.a ) * xTmp + Math.cos( refSpace.a ) * yTmp);

    ////////////
    // angle computation in the new space (referential space)
    ////////////
    double theta = 0.0;
    double sq = xTarget_rob * xTarget_rob + yTarget_rob *yTarget_rob;

    if( sq != 0.0f) theta = Math.asin( yTarget_rob / Math.sqrt( sq ) );

    theta = theta % Math.PI;
    if( theta > Math.PI ) theta -= 2.0f * Math.PI;
    if( theta < -Math.PI ) theta += 2.0f * Math.PI;

    // apply good sign to theta
    if( xTarget_rob >= 0.0f )
      if( yTarget_rob <= 0.0f )
        theta = Math.abs(theta);
      else theta = -Math.abs(theta);
    else
      if( yTarget_rob <= 0.0f )
        theta = (Math.PI - Math.abs( theta ));
      else theta = (-Math.PI + Math.abs( theta ));

    // singularity
    if( yTarget_rob == 0.0f && xTarget_rob < 0.0f ) 
      theta = Math.PI;

    relT.x = xTarget_rob;
    relT.y = yTarget_rob; 
    
    relT.a = theta;
    
    // distance computation with sign management
    relT.d = Math.sqrt( relT.x * relT.x + relT.y * relT.y);
    if( relT.x < 0.f) relT.d = - relT.d;
    
    return relT;
  }
  
  public double fEncoderWheelPerimeter;           // encoderWheelPerimeter Perimeter of the encoder wheel
  public double fEncoderWheelsDistanceToCenter;   // encoderWheelsDistance Distance from a encoder wheel to the robot center. Suppose the robot symetric.
  public int fEncoderTickPerRotation;              // encoderTickPerRotation Encoder precision
  public double fEncoderWheelDiameter;            // Encoder wheel diameter
  
  private volatile KeyPoint fCurrPosition;        // current position of the robot in the field space
  private int fEncLeft_prev;                      // Last left encoder values
  private int fEncRight_prev;                     // Last right encoder values
	@Override
	public void finish() {
		// TODO Auto-generated method stub
		
	}
}
